/**
 * @file 	 eura.c
 * @brief    欧瑞传动伺服驱动器 SD20X 系列驱动
 * @author 	 wenbo
 * @version  1.0
 * @date 	 2022-04-22
 *
 * @copyright Copyright (c) 2022  YGL
 *
 */

#include "eura_sd20x.h"
#include "stdlib.h"
#include "string.h"
#include "boardio.h"

static const uint16_t REG_COMMUNICATION_READ_AND_WRITE_PERMISSION = 505;	// 通讯读写准许 (Po505, 0 ~ 1, N/A, 1, 立即生效)
static const uint16_t REG_CONTROL_MODE_AND_FORWARDREVERSE_DIRECTION_SETTING = 1;	// 控制模式及正反转方向设定 (Po001, 两参数 ~ 两参数, N/A, d 121, 重新上电)
static const uint16_t REG_INTERNAL_POSITION_VEL = 330;	// 内部位置 1 速度 (Po330, 0 ~ 32000, 0.1rpm, 0, 立即生效)
static const uint16_t REG_INTERNAL_POSITION_GIVEN_SPEED_UNIT = 338;	// 内部位置给定速度单位 (Po338, 0 ~ 1, N/A, 0, 立即生效)
static const uint16_t REG_MULTI_SEGMENT_INTERNAL_POSITION_SETTING = 348;	// 多段内部位置设定 (Po348, 两参数 ~ 两参数, N/A, d 2 0, 立即生效)
static const uint16_t REG_INTERNAL_POSITION_REFERENCE_1 = 350;	// 内部位置 1 给定 (Po350, -2147483647 ~ 2147483647, N/A, 0, 立即生效)
static const uint16_t REG_INTERNAL_POSITION_CTRL_MODE = 341;	// 内部位置控制模式 (Po341, 0 ~ 1, N/A, 0, 立即生效)
static const uint16_t REG_INTERNAL_POSITION_TRIGGER = 342;	// 内部位置触发 (Po342, 0 ~ 1, N/A, 0, 立即生效)
static const uint16_t REG_DI_FUNCTION = 412;	// DI 功能 (Po412, 0 ~ 6, N/A, 0, 立即生效)
static const uint16_t REG_SERVO_STSTEM_CTRL = 99;	// 伺服系统功能控制 (Po099, 0 ~ 10, N/A, 0, 立即生效)
static const uint16_t REG_POSITION_ERROR = 912;	// 指令脉冲偏差计数 (Po912, -2147483647 ~ 2147483647, N/A, 0, 立即生效)
static const uint16_t REG_ALARM_CODE = 940;	// 报警代码 (Po100, 0 ~ 1, N/A, 0, 立即生效)
static const uint16_t REG_SERVO_INTERNAL_ENABLE = 100;	// 伺服内部使能 (Po100, 0 ~ 1, N/A, 0, 立即生效)
static const uint16_t REG_FORWARD_ROTATION_PROHIBITION_SETTING = 817;	// 正转禁止设置 (So-17, 0 ~ 1, N/A, 1, 立即生效)
static const uint16_t REG_REVERSE_PROHIBITION_SETTING = 818;	// 反转禁止设置 (So-18, 0 ~ 1, N/A, 1, 立即生效)
static int xp_eura_read_reginster(eura_sd20_t* const handler, uint16_t addr, uint16_t read_len, uint8_t* rec_data);
static int xp_eura_write_reginster(eura_sd20_t* const handler, uint16_t addr, uint8_t len, uint16_t* data);
bool config_internal_pos_mode(eura_sd20_t* const handle);

#if defined(ENABLE_ALARM_CHINESE_OUTPUT) && (ENABLE_ALARM_CHINESE_OUTPUT)
const char ALARM_TABLE[][40] = {
    "过流",
    "过压",
    "欠压",
    "硬件错误",
    "电角度识别错误",
    "电机过载",
    "超速",
    "驱动器过载",
    "位置环跟踪误差过大",
    "编码器故障",
    "紧急停止",
    "驱动器过热",
    "主电路电源缺相",
    "能耗制动错误",
    "——",
    "输入端子设置重复",
    "编码器断线",
    "转动惯量识别错误",
    "编码器电池警告",
    "伺服电机E2ROM未初始化",
    "零漂过大",
    "增量编码器Z相信号缺失",
    "转矩失调保护",
    "编码器电池报警",
    "电机过热",
    "电机温度检测断线",
    "超程保护",
    "E2ROM错误",
    "—",
    "堵转保护",
    "全闭环混合误差",
    "龙门同步错误",
    "电子凸轮错误",
    "PLC",
    "找原点超时错误",
    "参数拷贝错误",
    "未检测到高阻态",
    "UVW",
    "绝对值编码器分辨率错误",
    "绝对值编码器超速报警",
    "—",
    "主电掉电"
};
#endif


/**
 * @brief       此结构由上层定义并初始化，这里仅作为展示用
 */
static eura_sd20_t eura_sd20 = {
    .com.port = 1,
    .com.slave_id = 1,
    .com.speed = 9600,
};


bool xp_eura_init(eura_sd20_t* const handle)
{
    //初始化端口modbus
    int ret = xp_modbus_init(handle->com.port, handle->com.speed, MB_PAR_NONE, 100);
    if (0 == ret) {
        if (!config_internal_pos_mode(handle)) {
            printf("config_internal_pos_mode error!\n");
            return false;
        }
        handle->isInitOk = true;
    }
}

bool xp_eura_Deinit(eura_sd20_t* const handle)
{
    handle->isInitOk = false;
    return true;
}

/**
 * @brief       获取时间戳接口
 * @return      uint64_t            时间戳ms
 */
static uint64_t _xp_eura_get_timestamp(void)
{
    return aos_now_ms();
}
/**
 * @brief       读取控制模式及正反转方向设定
 * @param[out]	pResult             两参数
 * @return      bool                true: 成功 false: 通信故障
 */
bool get_control_mode_and_forwardreverse_direction_setting(eura_sd20_t* const handle, uint16_t* pResult){
    return xp_eura_read_reginster(handle, REG_CONTROL_MODE_AND_FORWARDREVERSE_DIRECTION_SETTING, 1, (uint8_t*)pResult) == 0 ? true : false;
}
/**
 * @brief       设置控制模式及正反转方向设定
 * @param[in]	data                两参数
 * @return      bool                true: 成功 false: 通信故障
 */
bool set_control_mode_and_forwardreverse_direction_setting(eura_sd20_t* const handle, uint16_t data){
    return xp_eura_write_reginster(handle, REG_CONTROL_MODE_AND_FORWARDREVERSE_DIRECTION_SETTING, 1, &data) == 0 ? true : false;
}
/**
 * @brief       设置DI功能
 * @param[in]	data                两参数
 * @return      bool                true: 成功 false: 通信故障
 */
bool set_di_function(eura_sd20_t* const handle, uint16_t data){
    return xp_eura_write_reginster(handle, REG_DI_FUNCTION, 1, &data) == 0 ? true : false;
}
/**
 * @brief       读取DI功能
 * @param[in]	data                两参数
 * @return      bool                true: 成功 false: 通信故障
 */
bool get_di_function(eura_sd20_t* const handle, uint16_t* pResult){
    return xp_eura_read_reginster(handle, REG_DI_FUNCTION, 1, (uint8_t*)pResult) == 0 ? true : false;
}
/**
 * @brief       设置内部位置 1 速度
 * @param[in]	data                0 ~ 32000
 * @return      bool                true: 成功 false: 通信故障
 */
bool set_internal_position_vel1(eura_sd20_t* const handle, uint16_t data){
    return xp_eura_write_reginster(handle, REG_INTERNAL_POSITION_VEL, 1, &data) == 0 ? true : false;
}
/**
 * @brief       设置内部位置 2 速度
 * @param[in]	data                0 ~ 32000
 * @return      bool                true: 成功 false: 通信故障
 */
bool set_internal_position_vel2(eura_sd20_t* const handle, uint16_t data){
    return xp_eura_write_reginster(handle, REG_INTERNAL_POSITION_VEL + 1, 1, &data) == 0 ? true : false;
}
/**
 * @brief       读取内部位置给定速度单位
 * @param[out]	pResult             0 ~ 1
 * @return      bool                true: 成功 false: 通信故障
 */
bool get_internal_position_given_speed_unit(eura_sd20_t* const handle, uint16_t* pResult){
    return xp_eura_read_reginster(handle, REG_INTERNAL_POSITION_GIVEN_SPEED_UNIT, 1, (uint8_t*)pResult) == 0 ? true : false;
}
/**
 * @brief       设置内部位置给定速度单位
 * @param[in]	data                0 ~ 1
 * @return      bool                true: 成功 false: 通信故障
 */
bool set_internal_position_given_speed_unit(eura_sd20_t* const handle, uint16_t data){
    return xp_eura_write_reginster(handle, REG_INTERNAL_POSITION_GIVEN_SPEED_UNIT, 1, &data) == 0 ? true : false;
}
/**
 * @brief       设置伺服内部使能
 * @param[in]	data                0 ~ 1
 * @return      bool                true: 成功 false: 通信故障
 */
bool set_servo_internal_enable(eura_sd20_t* const handle, uint16_t data){
    return xp_eura_write_reginster(handle, REG_SERVO_INTERNAL_ENABLE, 1, &data) == 0 ? true : false;
}
/**
 * @brief       设置内部位置触发
 * @param[in]	data                0 ~ 1
 * @return      bool                true: 成功 false: 通信故障
 */
bool set_internal_position_trigger(eura_sd20_t* const handle, uint16_t data){
    return xp_eura_write_reginster(handle, REG_INTERNAL_POSITION_TRIGGER, 1, &data) == 0 ? true : false;
}
/**
 * @brief       读取多段内部位置设定
 * @param[out]	pResult             两参数
 * @return      bool                true: 成功 false: 通信故障
 */
bool get_multi_segment_internal_position_setting(eura_sd20_t* const handle, uint16_t* pResult){
    return xp_eura_read_reginster(handle, REG_MULTI_SEGMENT_INTERNAL_POSITION_SETTING, 1, (uint8_t*)pResult) == 0 ? true : false;
}
/**
 * @brief       读取多段内部位置设定
 * @param[out]	pResult             -2147483647 ~ 2147483647
 * @return      bool                true: 成功 false: 通信故障
 */
bool get_position_error(eura_sd20_t* const handle, uint32_t* pResult){
    return xp_eura_read_reginster(handle, REG_POSITION_ERROR, 2, (uint8_t*)pResult) == 0 ? true : false;
}
/**
 * @brief       设置内部位置 2 给定
 * @param[in]	data                -2147483647 ~ 2147483647
 * @return      bool                true: 成功 false: 通信故障
 */
bool set_internal_position_reference_2(eura_sd20_t* const handle, uint32_t data){
    return xp_eura_write_reginster(handle, REG_INTERNAL_POSITION_REFERENCE_1 + 2, 2, (uint16_t*)&data);
}
/**
 * @brief       设置多段内部位置设定
 * @param[in]	data                两参数
 * @return      bool                true: 成功 false: 通信故障
 */
bool set_multi_segment_internal_position_setting(eura_sd20_t* const handle, uint16_t data){
    return xp_eura_write_reginster(handle, REG_MULTI_SEGMENT_INTERNAL_POSITION_SETTING, 1, &data) == 0 ? true : false;
}
/**
 * @brief       读取内部位置控制模式
 * @param[out]	pResult             0 ~ 1
 * @return      bool                true: 成功 false: 通信故障
 */
bool get_internal_position_ctrl_mode(eura_sd20_t* const handle, uint16_t* pResult){
    return xp_eura_read_reginster(handle, REG_INTERNAL_POSITION_CTRL_MODE, 1, (uint8_t*)pResult) == 0 ? true : false;
}
/**
 * @brief       读取508
 * @param[out]	pResult             0 ~ 1
 * @return      bool                true: 成功 false: 通信故障
 */
bool get_508(eura_sd20_t* const handle, uint16_t* pResult){
    return xp_eura_read_reginster(handle, 2, 1, (uint8_t*)pResult) == 0 ? true : false;
}
/**
 * @brief       设置508
 * @param[in]	data                0 ~ 1
 * @return      bool                true: 成功 false: 通信故障
 */
bool set_508(eura_sd20_t* const handle, uint16_t data){
    return xp_eura_write_reginster(handle, 508, 1, &data) == 0 ? true : false;
}
/**
 * @brief       读取电机代码
 * @param[out]	pResult             0 ~ 1
 * @return      bool                true: 成功 false: 通信故障
 */
bool get_motor_code2(eura_sd20_t* const handle, uint16_t* pResult){
    return xp_eura_read_reginster(handle, 0x02, 1, (uint8_t*)pResult) == 0 ? true : false;
}
/**
 * @brief       设置正转禁止设置
 * @param[in]	data                0 ~ 1
 * @return      bool                true: 成功 false: 通信故障
 */
bool set_forward_rotation_prohibition_setting(eura_sd20_t* const handle, uint16_t data){
    return xp_eura_write_reginster(handle, REG_FORWARD_ROTATION_PROHIBITION_SETTING, 1, &data) == 0 ? true : false;
}
/**
 * @brief       设置反转禁止设置
 * @param[in]	data                0 ~ 1
 * @return      bool                true: 成功 false: 通信故障
 */
bool set_reverse_prohibition_setting(eura_sd20_t* const handle, uint16_t data){
    return xp_eura_write_reginster(handle, REG_REVERSE_PROHIBITION_SETTING, 1, &data) == 0 ? true : false;
}

/**
 * @brief       设置正转禁止设置
 * @param[in]	data                0 ~ 1
 * @return      bool                true: 成功 false: 通信故障
 */
bool get_forward_rotation_prohibition_setting(eura_sd20_t* const handle, uint16_t* pResult){
    return xp_eura_read_reginster(handle, REG_FORWARD_ROTATION_PROHIBITION_SETTING, 1, (uint8_t*)pResult) == 0 ? true : false;
}
/**
 * @brief       读取报警代码
 * @param[in]	data                0 ~ 1
 * @return      bool                true: 成功 false: 通信故障
 */
bool get_alarm_code(eura_sd20_t* const handle, uint64_t* pResult){
    return xp_eura_read_reginster(handle, REG_ALARM_CODE, 4, (uint8_t*)pResult) == 0 ? true : false;
}
/**
 * @brief       设置反转禁止设置
 * @param[in]	data                0 ~ 1
 * @return      bool                true: 成功 false: 通信故障
 */
bool get_reverse_prohibition_setting(eura_sd20_t* const handle, uint16_t* pResult){
    return xp_eura_read_reginster(handle, REG_REVERSE_PROHIBITION_SETTING, 1, (uint8_t*)pResult) == 0 ? true : false;
}

/**
 * @brief       设置内部位置控制模式
 * @param[in]	data                0 ~ 1
 * @return      bool                true: 成功 false: 通信故障
 */
bool set_internal_position_ctrl_mode(eura_sd20_t* const handle, uint16_t data){
    return xp_eura_write_reginster(handle, REG_INTERNAL_POSITION_CTRL_MODE, 1, &data) == 0 ? true : false;
}
/**
 * @brief       设置通讯读写准许
 * @param[in]	data                0 ~ 1
 * @return      bool                true: 成功 false: 通信故障
 */
bool set_communication_read_and_write_permission(eura_sd20_t* const handle, uint16_t data){
    return xp_eura_write_reginster(handle, REG_COMMUNICATION_READ_AND_WRITE_PERMISSION, 1, &data) == 0 ? true : false;
}
/**
 * @brief       设置伺服系统控制
 * @param[in]	data                0 ~ 10
 * @return      bool                true: 成功 false: 通信故障
 */
bool set_servo_ststem_ctrl(eura_sd20_t* const handle, uint16_t data){
    return xp_eura_write_reginster(handle, REG_SERVO_STSTEM_CTRL, 1, &data) == 0 ? true : false;
}
/**
 * @brief       读取相对位置反馈
 * @param[out]	pResult             
 * @return      bool                true: 成功 false: 通信故障
 */
bool get_relative_pos_feedback(eura_sd20_t* const handle, uint32_t* pResult){
    return xp_eura_read_reginster(handle, 906, 2, (uint8_t*)pResult) == 0 ? true : false;
}
/**
 * @brief       读取绝对位置反馈
 * @param[out]	pResult             
 * @return      bool                true: 成功 false: 通信故障
 */
bool get_absolute_pos_feedback(eura_sd20_t* const handle, uint32_t* pResult){
    return xp_eura_read_reginster(handle, 954, 2, (uint8_t*)pResult) == 0 ? true : false;
}
/**
 * @brief       配置内部寄存器位置模式
 * @return      bool                true: 执行成功
 */
bool config_internal_pos_mode(eura_sd20_t* const handle)
{
    uint16_t data;
    bool is_restart = false;

    get_508(handle, &data);
    printf("max: %04X\r\n", data);

    // 设置允许参数写入EEROM
    CHECK_ERR(set_communication_read_and_write_permission(handle, 0), "set_communication_read_and_write_permission failed", err_1);

    CHECK_ERR(get_forward_rotation_prohibition_setting(handle, &data), "get_forward_rotation_prohibition_setting failed", err_1);
    if (data) is_restart = true;
    printf("get_forward_rotation_prohibition_setting: %04X\r\n", data);

    CHECK_ERR(get_reverse_prohibition_setting(handle, &data), "get_reverse_prohibition_setting failed", err_1);
    if (data) is_restart = true;
    printf("get_reverse_prohibition_setting: %04X\r\n", data);

    CHECK_ERR(set_forward_rotation_prohibition_setting(handle, 0), "set_forward_rotation_prohibition_setting failed", err_1);
    CHECK_ERR(set_reverse_prohibition_setting(handle, 0), "set_reverse_prohibition_setting failed", err_1);

    CHECK_ERR(get_di_function(handle, &data), "get_di_function failed", err_1);
    printf("DI POS sel 412: %04X\r\n", data);
    if (data != 0x1000)
    {
        data = 0x0010;
        CHECK_ERR(set_di_function(handle, data), "set_di_function failed", err_1);
        is_restart = true;
    }

    // 检测并配置控制模式为内部寄存器位置模式,配置完毕后需要重启生效
    CHECK_ERR(get_control_mode_and_forwardreverse_direction_setting(handle, &data), "get_control_mode_and_forwardreverse_direction_setting failed", err_1);
    printf("ctrl_mode: %04X\r\n", data);
    if ((data & 0xFF00) != 0x0500){        
        data = 0x0005;
        CHECK_ERR(set_control_mode_and_forwardreverse_direction_setting(handle, data), "set_control_mode_and_forwardreverse_direction_setting failed", err_1);
        is_restart = true;
    }
	CHECK_ERR(get_control_mode_and_forwardreverse_direction_setting(handle, &data), "get_control_mode_and_forwardreverse_direction_setting failed", err_1);
    printf("get ctrl_mode: %04X\r\n", data);

    // 检测并配置位置控制模式下给定速度单位
    CHECK_ERR(get_internal_position_given_speed_unit(handle, &data), "get_internal_position_given_speed_unit failed", err_1);
    printf("vel_unit: %04X\r\n", data);
    if (0 != data){
        data = 0;   // 0：电机实际转速，单位为 0.1r/min，与电子齿轮比无关；1：单位为 0.01KHz，经过电子齿轮分频处理之后
        CHECK_ERR(set_internal_position_given_speed_unit(handle, data), "set_internal_position_given_speed_unit failed", err_1);
    }

    // 检测并配置位置控制模式下多段内部位置设定
    CHECK_ERR(get_multi_segment_internal_position_setting(handle, &data), "get_multi_segment_internal_position_setting failed", err_1);
    printf("segment_mode: %04X\r\n", data);
    if (0x0002 != data){
        data = 0x0200;    // 2: 2段位置, 0: 不启用多段位置功能
        CHECK_ERR(set_multi_segment_internal_position_setting(handle, data), "set_multi_segment_internal_position_setting failed", err_1);
    }

    // 检测并配置位置模式为相对或绝对
    CHECK_ERR(get_internal_position_ctrl_mode(handle, &data), "get_internal_position_ctrl_mode failed", err_1);
    printf("pos_mode: %04X\r\n", data);
    if (0 != data){
        data = 0;   // 0: 相对模式, 1: 绝对模式
        CHECK_ERR(set_internal_position_ctrl_mode(handle, data), "set_internal_position_ctrl_mode failed", err_1);
    }
	
	xp_eura_read_reginster(handle, 339, 1, (uint8_t*)&data);
	printf("Po339= %d\r\n", data);
	
	xp_eura_read_reginster(handle, 344, 1, (uint8_t*)&data);
	printf("Po344= %d\r\n", data);
	
	xp_eura_read_reginster(handle, 346, 1, (uint8_t*)&data);
	printf("Po346= %d\r\n", data);
	
	xp_eura_read_reginster(handle, 304, 1, (uint8_t*)&data);
	printf("Po304= %d\r\n", data);
	
	xp_eura_read_reginster(handle, 305, 1, (uint8_t*)&data);
	printf("Po305= %d\r\n", data);
	
	xp_eura_read_reginster(handle, 119, 1, (uint8_t*)&data);
	printf("Po119= %d\r\n", data);
	
	uint64_t alarm;
	get_alarm_code(handle, &alarm);
	printf("alarm= %lld\r\n", alarm);
	
	data = 1;
	xp_eura_write_reginster(handle, 304, 1, &data);
	xp_eura_write_reginster(handle, 305, 1, &data);
	xp_eura_write_reginster(handle, 344, 1, &data);
	xp_eura_write_reginster(handle, 346, 1, &data);

	
	// 配置默认速度
	CHECK_ERR(set_internal_position_vel1(handle, 100), "set_internal_position_vel1 failed", err_1);
	CHECK_ERR(set_internal_position_vel2(handle, 100), "set_internal_position_vel2 failed", err_1);

    // 设置不允许参数写入EEROM
    CHECK_ERR(set_communication_read_and_write_permission(handle, 1), "set_communication_read_and_write_permission failed", err_1);

    if (is_restart){
        CHECK_ERR(set_servo_ststem_ctrl(handle, 0x0A), "set_servo_ststem_ctrl failed", err_1);
        println("restart servo");
        aos_msleep(3000);
    }

    set_servo_internal_enable(handle, 1);
    return true;
err_1:
    return false;
}

bool run_abs_pos(eura_sd20_t* const handle, int32_t abs_pos, uint16_t vel)
{
    uint16_t data;

    println("run_abs_pos: %ld, vel: %d", abs_pos, vel);
    if (handle->pos_ctrl_mode != 1){
        CHECK_ERR(set_internal_position_ctrl_mode(handle, 1), "set_internal_position_ctrl_mode failed", err_1);
        handle->pos_ctrl_mode = 1;
    }

	uint64_t alarm;
	if (get_alarm_code(handle, &alarm)){
		println("Alarm: %08X%08X", (uint32_t)alarm, (uint32_t)(alarm >> 32));
		if (0 != alarm){
			set_servo_ststem_ctrl(handle, 0x01);
			println("clear error");
			config_internal_pos_mode(handle);
		}
	}
	
	uint32_t cur_pos = 0;
	get_relative_pos_feedback(handle, &cur_pos);
	println("current rel pos: %08X, %ld, %ld", cur_pos, cur_pos, __rev(cur_pos));
    // 设置位置参考
	set_internal_position_reference_2(handle, abs_pos);

	// 设置位置速度
    CHECK_ERR(set_internal_position_vel2(handle, vel), "set_internal_position_vel2 failed", err_1);
//    if (handle->pos_mode_vel != vel){
//        
//        handle->pos_mode_vel = vel;
//    }
    // 设置位置触发
    CHECK_ERR(set_internal_position_trigger(handle, 1), "set_internal_position_trigger failed", err_1);
    return true;
err_1:
    return false;
}
bool run_rel_pos(eura_sd20_t* const handle, int32_t rel_pos, uint16_t vel)
{
    uint16_t data;

    println("run_rel_pos: %ld, vel: %d", rel_pos, vel);
    if (handle->pos_ctrl_mode != 0){
        CHECK_ERR(set_internal_position_ctrl_mode(handle, 0), "set_internal_position_ctrl_mode failed", err_1);
        handle->pos_ctrl_mode = 0;
    }
	
	uint64_t alarm;
	if (get_alarm_code(handle, &alarm)){
		println("Alarm: %08X%08X", (uint32_t)alarm, (uint32_t)(alarm >> 32));
		if (0 != alarm){
			set_servo_ststem_ctrl(handle, 0x01);
			println("clear error");
			config_internal_pos_mode(handle);
		}
	}
	
	uint32_t cur_pos = 0;
	get_relative_pos_feedback(handle, &cur_pos);
	println("current rel pos: %08X, %ld, %ld", cur_pos, cur_pos, __rev(cur_pos));
	
	// 设置位置速度
    CHECK_ERR(set_internal_position_vel2(handle, vel), "set_internal_position_vel2 failed", err_1);
	
    // 设置位置参考
	set_internal_position_reference_2(handle, rel_pos);
	
//    if (handle->pos_mode_vel != vel){
//        // 设置位置速度
//        CHECK_ERR(set_internal_position_vel2(handle, vel), "set_internal_position_vel failed", err_1);
//        handle->pos_mode_vel = vel;
//    }
    // 设置位置触发
    CHECK_ERR(set_internal_position_trigger(handle, 1), "set_internal_position_trigger failed", err_1);
    return true;
err_1:
    return false;
}
bool go_to_zero(eura_sd20_t* const handle)
{
	uint16_t data;
	uint64_t alarm;
	if (get_alarm_code(handle, &alarm)){
		printf("Alarm: %08X%08X", (uint32_t)alarm, (uint32_t)(alarm >> 32));
		if (0 != alarm){
			set_servo_ststem_ctrl(handle, 0x01);
			println("clear error");
			config_internal_pos_mode(handle);
		}
	}
	data = 3;
    return 0 == xp_eura_write_reginster(handle, 125, 3, &data) ? true : false;
}
bool pos_is_arrived(eura_sd20_t* const handle, uint32_t *pResult)
{
    CHECK_ERR(get_position_error(handle, pResult), "get_position_error failed", err_1);
    return true;
err_1:
    return false;
}
static int xp_eura_write_reginster(eura_sd20_t* const handler, uint16_t addr, uint8_t len, uint16_t* data)
{
    int sta = 0;
    MB_data modbus;
    uint8_t try_times = 3;

    modbus.slave_id = handler->com.slave_id;
	if (len > 1){
		modbus.mb_fun = mb_hold_w;
	}else{
		modbus.mb_fun = mb_single_w;
	}
    
    modbus.start_addr = addr;
    modbus.wBuf = data;
    modbus.rwLen = len;
    modbus.resBuf = handler->com.rec_data;
    modbus.resLen = &handler->com.rec_len;
    while (try_times--) {
        if (0 == xp_modbus_ask(handler->com.port, &modbus)) return 0;
    }

    return -1;
}

static int xp_eura_read_reginster(eura_sd20_t* const handler, uint16_t addr, uint16_t read_len, uint8_t* rec_data)
{
    int sta = 0;
    MB_data modbus;
    uint8_t try_times = 3;
    uint8_t rec_len;

    modbus.slave_id = handler->com.slave_id;
    modbus.mb_fun = mb_hold_r;
    modbus.start_addr = addr;
    modbus.rwLen = read_len;
    modbus.resBuf = rec_data;
    modbus.resLen = &rec_len;
    while (try_times--) {
        if (0 == xp_modbus_ask(handler->com.port, &modbus)) {
//            if (1 == read_len){
//                uint8_t tmp = rec_data[0];
//                rec_data[0] = rec_data[1];
//                rec_data[1] = tmp;
//            }
//            else if (2 == read_len){
//                uint8_t tmp = rec_data[0];
//                rec_data[0] = rec_data[3];
//                rec_data[3] = tmp;
//                tmp = rec_data[1];
//                rec_data[1] = rec_data[2];
//                rec_data[2] = tmp;
//            }
            return 0;
        }
    }
    printf("eura read fail !!!");
    return -1;
}
//带大小端转换的memcpy  __REVSH
void memcpy_resv_word(void *des, void * const res, uint16_t size)
{
	for (uint16_t i = 0; i < size; i += sizeof(uint32_t)){
		*((uint8_t *)des + i + 0) = *((uint8_t *)res + i + 3);
		*((uint8_t *)des + i + 1) = *((uint8_t *)res + i + 2);
		*((uint8_t *)des + i + 2) = *((uint8_t *)res + i + 1);
		*((uint8_t *)des + i + 3) = *((uint8_t *)res + i + 0);
	}
}

//输出各参数
void xp_eura_show(eura_sd20_t* const handler)
{
    printf("*****************************************\n");

    printf("pos_mode_vel:         %d\n", handler->pos_mode_vel);
   
    printf("*****************************************\n\n");
}
